﻿using MaterialDesignThemes.Wpf;
using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using MatlabWork.Entity;
using Prism.Mvvm;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using TestingPlatForm.MissionContrl.Entity;

namespace TestingPlatForm.MissionContrl.Models
{
    public class CalculateRAH : BindableBase
    {

        public CalculateRAH()
        {
        }

        public List<numTest> Calculate(List<numTest> numlist)
        {
            List<numTest> EALists = new List<numTest>();
            numlist.ForEach(num => {
            numTest numTest = num;
            Vector<double> X = Vector<double>.Build.DenseOfArray(new double[] { GlobalEntity.configData.x_bs / 1000, GlobalEntity.configData.y_bs / 1000, GlobalEntity.configData.z_bs / 1000 });
            Vector<double> RV = Vector<double>.Build.DenseOfArray(new double[] { num.x / 1000, num.y / 1000, num.z / 1000, num.vx / 1000, num.vy / 1000, num.vz / 1000 });
            Vector<double> Sat2SH_ECEF = Vector<double>.Build.DenseOfArray(new double[] { X[0] - RV[0], X[1] - RV[1], X[2] - RV[2] });
            Vector<double> Sat2SH_VVLH = ECEF2VVLH(1, RV, Sat2SH_ECEF);
            double temp = Math.Atan2(Sat2SH_VVLH[1], Sat2SH_VVLH[0]) * 180 / Math.PI;
            numTest.ElevationDown = Math.Round(Math.Acos(Sat2SH_VVLH[2] / CalculateEuclideanNorm(Sat2SH_VVLH)) * 180 / Math.PI, 4);
            // rah.R = rah.A - 90;
                numTest.ElevationUp = (Math.Acos(Math.Sin(numTest.ElevationDown * Math.PI / 180) * (6378.137 + 800) / 6378.137) * 180 / Math.PI) % 90;
                numTest.Azimuth = Math.Round((temp + 360) % 360, 4);
                numTest.lu_distance = Math.Sqrt((num.x  - GlobalEntity.configData.x_st1) * (num.x - GlobalEntity.configData.x_st1) + (num.y - GlobalEntity.configData.y_st1) * (num.y - GlobalEntity.configData.y_st1) + (num.z - GlobalEntity.configData.z_st1) * (num.z - GlobalEntity.configData.z_st1)) /1000;
                numTest.lf_distance = Math.Sqrt((num.x - GlobalEntity.configData.x_bs) * (num.x - GlobalEntity.configData.x_bs) + (num.y - GlobalEntity.configData.y_bs) * (num.y - GlobalEntity.configData.y_bs) + (num.z - GlobalEntity.configData.z_bs) * (num.z - GlobalEntity.configData.z_bs)) /1000;
                numTest.ku_up = 20 * Math.Log10(GlobalEntity.configData.fu_up * 1000) + 20 * Math.Log10(numTest.lu_distance) + 32.4;
                numTest.ku_down = 20 * Math.Log10(GlobalEntity.configData.fu_down * 1000) + 20 * Math.Log10(numTest.lu_distance) + 32.4;
                numTest.qv_up = 20 * Math.Log10(GlobalEntity.configData.ff_up * 1000) + 20 * Math.Log10(numTest.lf_distance) + 32.4;
                numTest.qv_down = 20 * Math.Log10(GlobalEntity.configData.ff_down * 1000) + 20 * Math.Log10(numTest.lf_distance) + 32.4;
                numTest.qv_sway = 20 * Math.Log10(GlobalEntity.configData.ff_sway * 1000) + 20 * Math.Log10(numTest.lf_distance) + 32.4;
                EALists.Add(numTest);
            });
            return EALists;
        }

        public List<downLinkData> CalculateDownLink(List<numTest> numlist)
        {
            List<downLinkData> EALists = new List<downLinkData>();
            numlist.ForEach(num => {
                downLinkData downLinkData = new downLinkData();
                Vector<double> X = Vector<double>.Build.DenseOfArray(new double[] { GlobalEntity.configData.x_bs / 1000, GlobalEntity.configData.y_bs / 1000, GlobalEntity.configData.z_bs / 1000 });
                Vector<double> RV = Vector<double>.Build.DenseOfArray(new double[] { num.x / 1000, num.y / 1000, num.z / 1000, num.vx / 1000, num.vy / 1000, num.vz / 1000 });
                Vector<double> Sat2SH_ECEF = Vector<double>.Build.DenseOfArray(new double[] { X[0] - RV[0], X[1] - RV[1], X[2] - RV[2] });
                Vector<double> Sat2SH_VVLH = ECEF2VVLH(1, RV, Sat2SH_ECEF);
                double temp = Math.Atan2(Sat2SH_VVLH[1], Sat2SH_VVLH[0]) * 180 / Math.PI;
                double gt = 0;
                double elevationPre = Math.Round(Math.Acos(Sat2SH_VVLH[2] / CalculateEuclideanNorm(Sat2SH_VVLH)) * 180 / Math.PI, 4);
                downLinkData.ElevationDown = (Math.Acos(Math.Sin(elevationPre * Math.PI / 180) * (6378.137 + 800) / 6378.137) * 180 / Math.PI) % 90;
                foreach (DownData item in GlobalEntity.downDatas)
                {
                    if (item.Evaluation == Math.Round(elevationPre))
                    {
                        downLinkData.UPEIRP = item.EIRP;
                    }
                }

                foreach (UpData item in GlobalEntity.upDatas)
                {
                    if (item.Evaluation == Math.Round(downLinkData.ElevationDown))
                    {
                        gt = item.GT;
                    }
                }
                downLinkData.lu_distance = Math.Sqrt((num.x - GlobalEntity.configData.x_st1) * (num.x - GlobalEntity.configData.x_st1) + (num.y - GlobalEntity.configData.y_st1) * (num.y - GlobalEntity.configData.y_st1) + (num.z - GlobalEntity.configData.z_st1) * (num.z - GlobalEntity.configData.z_st1)) / 1000;
                downLinkData.lf_distance = Math.Sqrt((num.x - GlobalEntity.configData.x_bs) * (num.x - GlobalEntity.configData.x_bs) + (num.y - GlobalEntity.configData.y_bs) * (num.y - GlobalEntity.configData.y_bs) + (num.z - GlobalEntity.configData.z_bs) * (num.z - GlobalEntity.configData.z_bs)) / 1000;
                downLinkData.kuDown = 20 * Math.Log10(GlobalEntity.configData.fu_down * 1000) + 20 * Math.Log10(downLinkData.lu_distance) + 32.4;
                downLinkData.QVUp = 20 * Math.Log10(GlobalEntity.configData.ff_up * 1000) + 20 * Math.Log10(downLinkData.lf_distance) + 32.4;
                downLinkData.InPower = GlobalEntity.LinkData.IFpower + 95.4 - (30 - GlobalEntity.LinkData.LXgain) - GlobalEntity.LinkData.XVattenuation + (GlobalEntity.LinkData.SendGain - 5) + (GlobalEntity.LinkData.receiveGain - 0);
                downLinkData.EIRP = downLinkData.InPower + 55.98 - 30;
                downLinkData.SNR = downLinkData.EIRP - downLinkData.QVUp + 39.95 - GlobalEntity.configData.ff_up_ot - 10 * Math.Log10(273 + 40) - 10 * Math.Log10(GlobalEntity.LinkData.LinkWay) - 60 + 228.6;
                downLinkData.receiveGain = Math.Round(gt + 10 * Math.Log10(273 + 40) + 6 + 30, 2);
                downLinkData.IFpower = downLinkData.UPEIRP - downLinkData.kuDown - GlobalEntity.configData.fu_down_ot - GlobalEntity.LinkData.receptionLoss - GlobalEntity.LinkData.Lattenuation + 45 + Math.Round(gt + 10 * Math.Log10(273 + 40) + 6 + 30, 2) + 30;
                downLinkData.downSNR = Math.Round((downLinkData.UPEIRP - downLinkData.kuDown - GlobalEntity.configData.fu_down_ot + 228.6 - 10 * Math.Log10(GlobalEntity.LinkData.LinkWay) - 60 + gt), 2);
                downLinkData.dryingRatio = Math.Round(-10 * Math.Log10(Math.Pow(10,(- downLinkData.downSNR / 10)) + Math.Pow(10,(- downLinkData.SNR / 10))), 2);
                downLinkData.Time = num.Time;
                EALists.Add(downLinkData);
            });
            return EALists;
        }


        public List<UpLinkData> CalculateUPLink(List<numTest> numlist)
        {
            List<UpLinkData> EALists = new List<UpLinkData>();
            numlist.ForEach(num => {
                UpLinkData upLinkData = new UpLinkData();
                Vector<double> X = Vector<double>.Build.DenseOfArray(new double[] { GlobalEntity.configData.x_bs / 1000, GlobalEntity.configData.y_bs / 1000, GlobalEntity.configData.z_bs / 1000 });
                Vector<double> RV = Vector<double>.Build.DenseOfArray(new double[] { num.x / 1000, num.y / 1000, num.z / 1000, num.vx / 1000, num.vy / 1000, num.vz / 1000 });
                Vector<double> Sat2SH_ECEF = Vector<double>.Build.DenseOfArray(new double[] { X[0] - RV[0], X[1] - RV[1], X[2] - RV[2] });
                Vector<double> Sat2SH_VVLH = ECEF2VVLH(1, RV, Sat2SH_ECEF);
                double temp = Math.Atan2(Sat2SH_VVLH[1], Sat2SH_VVLH[0]) * 180 / Math.PI;
                double gain = 0;
                double gt = 0;
                double elevationPre = Math.Round(Math.Acos(Sat2SH_VVLH[2] / CalculateEuclideanNorm(Sat2SH_VVLH)) * 180 / Math.PI, 4);
                upLinkData.ElevationDown = (Math.Acos(Math.Sin(elevationPre * Math.PI / 180) * (6378.137 + 800) / 6378.137) * 180 / Math.PI) % 90;
                foreach (DownData item in GlobalEntity.downDatas)
                {
                    if (item.Evaluation == Math.Round(elevationPre))
                    {
                        gt = item.GT; break;
                    }
                }
                foreach (UpData item in GlobalEntity.upDatas)
                {
                    if (item.Evaluation == Math.Round(upLinkData.ElevationDown))
                    {
                        gain = item.gain; 
                    }
                }
                upLinkData.lu_distance = Math.Sqrt((num.x - GlobalEntity.configData.x_st1) * (num.x - GlobalEntity.configData.x_st1) + (num.y - GlobalEntity.configData.y_st1) * (num.y - GlobalEntity.configData.y_st1) + (num.z - GlobalEntity.configData.z_st1) * (num.z - GlobalEntity.configData.z_st1)) / 1000;
                upLinkData.lf_distance = Math.Sqrt((num.x - GlobalEntity.configData.x_bs) * (num.x - GlobalEntity.configData.x_bs) + (num.y - GlobalEntity.configData.y_bs) * (num.y - GlobalEntity.configData.y_bs) + (num.z - GlobalEntity.configData.z_bs) * (num.z - GlobalEntity.configData.z_bs)) / 1000;
                upLinkData.QVDown = 20 * Math.Log10(GlobalEntity.configData.ff_down * 1000) + 20 * Math.Log10(upLinkData.lf_distance) + 32.4;
                upLinkData.KuUp = 20 * Math.Log10(GlobalEntity.configData.fu_up * 1000) + 20 * Math.Log10(upLinkData.lu_distance) + 32.4;
                upLinkData.EIRP = true ? (GlobalEntity.LinkData.inputPower + 28 - GlobalEntity.LinkData.conversionLoss - GlobalEntity.LinkData.cableLoss + gain - 30): -99;
                upLinkData.FPD = Math.Round((upLinkData.EIRP - GlobalEntity.configData.fu_up_ot - 10 * Math.Log10(4 * Math.PI * Math.Pow((upLinkData.lf_distance * 1000) , 2))), 2);
                upLinkData.QVEIRP = Math.Round(Math.Min(upLinkData.FPD - -95 + 42 + 20 * Math.Log10(upLinkData.lu_distance / 800), 45), 2);
                upLinkData.receiveSNR = Math.Round(GlobalEntity.LinkData.inputPower + 28 - GlobalEntity.LinkData.cableLoss + gain - GlobalEntity.LinkData.conversionLoss - upLinkData.KuUp - GlobalEntity.configData.fu_up_ot + 228.6 - 10 * Math.Log10(GlobalEntity.LinkData.downLinkWay) - 60 + gt - 30, 2);
                upLinkData.IFpower = upLinkData.QVEIRP - upLinkData.QVDown - GlobalEntity.configData.ff_down_ot + 54.88 + 89.36 - GlobalEntity.LinkData.Xattenuation - (30 - GlobalEntity.LinkData.gain) + (GlobalEntity.LinkData.transmissionGain - 5) + GlobalEntity.LinkData.receptionGain + 30;
                upLinkData.downSNR = Math.Round(upLinkData.QVEIRP - upLinkData.QVDown - GlobalEntity.configData.ff_down_ot + 28.94 + 228.6 - 10 * Math.Log10(GlobalEntity.LinkData.downLinkWay) - 60, 2);
                upLinkData.dryRatio = Math.Round(-10 * Math.Log10(Math.Pow(10,(-upLinkData.downSNR / 10)) + Math.Pow(10,(-upLinkData.receiveSNR / 10))), 2);
                upLinkData.Time = num.Time;
                EALists.Add(upLinkData);
            });
            return EALists;
        }

        public List<SwayData> CalculateSway(List<numTest> numlist)
        {
            List<SwayData> SwayDatas = new List<SwayData>();
            numlist.ForEach(num => {
                SwayData swayData = new SwayData();
                Vector<double> X = Vector<double>.Build.DenseOfArray(new double[] { GlobalEntity.configData.x_bs / 1000, GlobalEntity.configData.y_bs / 1000, GlobalEntity.configData.z_bs / 1000 });
                Vector<double> RV = Vector<double>.Build.DenseOfArray(new double[] { num.x / 1000, num.y / 1000, num.z / 1000, num.vx / 1000, num.vy / 1000, num.vz / 1000 });
                Vector<double> Sat2SH_ECEF = Vector<double>.Build.DenseOfArray(new double[] { X[0] - RV[0], X[1] - RV[1], X[2] - RV[2] });
                Vector<double> Sat2SH_VVLH = ECEF2VVLH(1, RV, Sat2SH_ECEF);
                double temp = Math.Atan2(Sat2SH_VVLH[1], Sat2SH_VVLH[0]) * 180 / Math.PI;
                double ElevationDown = Math.Round(Math.Acos(Sat2SH_VVLH[2] / CalculateEuclideanNorm(Sat2SH_VVLH)) * 180 / Math.PI, 4);
                // rah.R = rah.A - 90;
                double ElevationUp = (Math.Acos(Math.Sin(ElevationDown * Math.PI / 180) * (6378.137 + 800) / 6378.137) * 180 / Math.PI) % 90;
                double lf_distance = Math.Sqrt((num.x - GlobalEntity.configData.x_bs) * (num.x - GlobalEntity.configData.x_bs) + (num.y - GlobalEntity.configData.y_bs) * (num.y - GlobalEntity.configData.y_bs) + (num.z - GlobalEntity.configData.z_bs) * (num.z - GlobalEntity.configData.z_bs)) / 1000;
                swayData.spaceAttenuation = Math.Round((20 * Math.Log10(GlobalEntity.configData.ff_sway) + 20 * Math.Log10(lf_distance * 1000) + 32.44), 2);
                swayData.IFpower = 26 - swayData.spaceAttenuation - 8 - 0 + 55.39 + 83.15 - 0 - 15 - (30 - 20) + (5 - 5) + 0 + 30;
                SwayDatas.Add(swayData);
            });
            return SwayDatas;
        }

        //public RAH Calcul(Vector<double> RV, Vector<double> X)
        //{
        //    Vector<double> Sat2SH_ECEF = Vector<double>.Build.DenseOfArray(new double[] { X[0] - RV[0], X[1] - RV[1], X[2] - RV[2] });
        //    Vector<double> Sat2SH_VVLH = ECEF2VVLH(1, RV, Sat2SH_ECEF);
        //    RAH rah = new RAH();
        //    double temp = Math.Atan2(Sat2SH_VVLH[1], Sat2SH_VVLH[0]) * 180 / Math.PI;
        //    rah.A = Math.Acos(Sat2SH_VVLH[2] / CalculateEuclideanNorm(Sat2SH_VVLH)) * 180 / Math.PI;
        //    rah.R = rah.A - 90;
        //    rah.H = (temp + 360) % 360;
        //    return rah;
        //}

        public static Vector<double> ECEF2VVLH(int Model, Vector<double> RV, Vector<double> X)
        {
            double We = 0.000072921150;
            Vector<double> RVTemp = Vector<double>.Build.DenseOfArray(RV.ToArray());

            // 速度修正
            if (Model > 0)
            {
                RVTemp[3] = RV[3] - RV[1] * We;
                RVTemp[4] = RV[4] + RV[0] * We;
            }

            // R 和 V
            Vector<double> R = RVTemp.SubVector(0, 3);
            Vector<double> V = RVTemp.SubVector(3, 3);

            // 计算所需向量和正规化
            double r = R.L2Norm();
            Vector<double> H = CrossProduct(R, V);
            double h = H.L2Norm();

            // 构建旋转矩阵
            Matrix<double> Roi = Matrix<double>.Build.Dense(3, 3);
            Roi.SetRow(2, (-R / r).ToArray());
            Roi.SetRow(1, (-H / h).ToArray());
            Roi.SetRow(0, CrossProduct(Roi.Row(1), Roi.Row(2)));

            // 应用模型转换
            if (Math.Abs(Model) == 1)
            {
                return Roi * X;
            }
            else if (Math.Abs(Model) == 2)
            {
                return Roi.Transpose() * X;
            }

            return null; // 如果模型不是 1 或者 -1, 2 或者 -2 返回null
        }

        public static Vector<double> CrossProduct(Vector<double> vector1, Vector<double> vector2)
        {
            // 叉积的计算公式：(v1y*v2z - v1z*v2y, v1z*v2x - v1x*v2z, v1x*v2y - v1y*v2x)
            Vector<double> result = Vector<double>.Build.DenseOfArray(new double[] { 0, 0, 0 }); ;
            result[0] = vector1[1] * vector2[2] - vector1[2] * vector2[1];
            result[1] = vector1[2] * vector2[0] - vector1[0] * vector2[2];
            result[2] = vector1[0] * vector2[1] - vector1[1] * vector2[0];
            return result;
        }

        public static double CalculateEuclideanNorm(Vector<double> vector)
        {
            double sumOfSquares = 0;

            foreach (double element in vector)
            {
                sumOfSquares += element * element;
            }

            return Math.Sqrt(sumOfSquares);
        }


        public numTest AZ2ECEF(double Longitude, double Latitude, numTest wGS_84)
        {
            Longitude = DegreesToRadians(Longitude);
            Latitude = DegreesToRadians(Latitude);
            /*Vector<double> Vec = Vector<double>.Build.DenseOfArray(new double[] { 0, 0, 0 });
            Vec[0] = wGS_84.Range * Math.Cos(DegreesToRadians(wGS_84.Elevation)) * Math.Sin(DegreesToRadians(wGS_84.Azimuth));
            Vec[1] = wGS_84.Range * Math.Cos(DegreesToRadians(wGS_84.Elevation)) * Math.Cos(DegreesToRadians(wGS_84.Azimuth));
            Vec[2] = wGS_84.Range * Math.Sin(DegreesToRadians(wGS_84.Elevation));*/
            Matrix<double> dr = Matrix<double>.Build.DenseOfArray(new double[,]
                   {
            {- Math.Sin(Longitude),  -Math.Cos(Longitude)* Math.Sin(Latitude), Math.Cos(Longitude) * Math.Cos(Latitude)},
            {Math.Cos(Longitude), -Math.Sin(Longitude) * Math.Sin(Latitude),Math.Sin(Longitude) * Math.Cos(Latitude)},
            {0, Math.Cos(Latitude) , Math.Sin(Latitude)}
                   });
            MathNet.Numerics.LinearAlgebra.Vector<double> Vec = MathNet.Numerics.LinearAlgebra.Vector<double>.Build.DenseOfArray(new double[] {
                wGS_84.Range * Math.Cos(DegreesToRadians(wGS_84.ElevationUp)) * Math.Sin(DegreesToRadians(wGS_84.Azimuth)),
                wGS_84.Range * Math.Cos(DegreesToRadians(wGS_84.ElevationUp)) * Math.Cos(DegreesToRadians(wGS_84.Azimuth)),
                wGS_84.Range * Math.Sin(DegreesToRadians(wGS_84.ElevationUp)) });

            Vector<double> result = dr * Vec;
            wGS_84.x = result[0];
            wGS_84.y = result[1];
            wGS_84.z = result[2];
            return wGS_84;
        }

        public static double DegreesToRadians(double degrees)
        {
            return degrees * (Math.PI / 180.0);
        }

        public static double RadiansToDegrees(double radians)
        {
            return radians * (180.0 / Math.PI);
        }

    }
}
